#include <opencv2/opencv.hpp>
#include "render/render.h"
#include "render/Engine/DrawingEngine.h"
#include "render/Objects/Renderer3DView.h"
#include "render/Objects/ImageRender.h"

using namespace pwp;

TinyRender::TinyRender() {
	width_ = 640, height_ = 480;
	render_view_ = new Renderer3DView(width_, height_, 640, 480, 502.299, 502.299, 319.5 , 239.5, 0.01f, 50.0f, 0);
	render_images_ = new ImageRender(width_, height_);
}

TinyRender::~TinyRender() {
	delete render_view_;
	delete render_images_;
}

void TinyRender::Render(
	Renderer3DObject& object, 
	const Eigen::Transform<float, 3, Eigen::Affine>& Tco, 
	cv::Mat* mask, cv::Mat* depth) 
{
	auto mat = Tco.matrix();
	float pose[16];
	for (int r = 0; r < mat.rows(); ++r)
		for (int c = 0; c < mat.cols(); ++c) {
			pose[c + r * 4] = mat(r, c);
		}

	DrawingEngine::Instance()->Draw(&object, render_view_, pose, render_images_);
	*mask = cv::Mat(height_, width_, CV_8U, render_images_->imageFill.pixels());
}

void TinyRender::Render(
	Renderer3DObject& object, const float* pa, 
	cv::Mat* mask, cv::Mat* depth) 
{
	DrawingEngine::Instance()->Draw(&object, render_view_, pa, render_images_);

	if (mask)
	*mask = cv::Mat(height_, width_, CV_8U, render_images_->imageFill.pixels());
}

bool TinyRender::CalcBodyPoint(
	const cv::Point2f& pt,
	const Eigen::Transform<float, 3, Eigen::Affine>& camera2body_pose,
	Eigen::Vector3f* pt_body)
{
	return CalcBodyPoint(pt.x, pt.y, camera2body_pose, pt_body);
}

bool TinyRender::CalcBodyPoint(
	int ix, int iy,
	const Eigen::Transform<float, 3, Eigen::Affine>& camera2body_pose,
	Eigen::Vector3f* pt_body)
{
#if 0
	int icx = ix, icy = iy;
	int icz = icx + icy * width_;
	auto zbuffer = render_images_->imageZBuffer;
	auto depth = zbuffer->pixels[icz];

	if (MAX_INT == depth)
		return false;

	Eigen::Vector3f pt_cam;
	pt_cam(0) = (float)2 * (icx - view_->renderView->view[0]) / view_->renderView->view[2] - 1.0f;
	pt_cam(1) = (float)2 * (icy - view_->renderView->view[1]) / view_->renderView->view[3] - 1.0f;
	pt_cam(2) = (float)2 * ((float)depth / (float)MAX_INT) - 1.0f;

	*pt_body = camera2body_pose * pt_cam;

	return true;
#endif
	float x_unproj[3];
	if (!DrawingEngine::Instance()->CalcBodyPoint(
		ix, iy, render_view_, render_images_, x_unproj))
		return false;

	Eigen::Vector3f pt_cam;
	pt_cam(0) = x_unproj[0];
	pt_cam(1) = x_unproj[1];
	pt_cam(2) = x_unproj[2];
	*pt_body = camera2body_pose * pt_cam;

	return true;
}